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1. Introduction 


Much recent work in robotics has been directed toward the development of 
task-level languages. An essential component of an interpreter for a task-level 
language is a geometric reasoning system which can detect collisions between the 
manipulator and objects in the workspace, and plan collision-free paths through 
the workspace. The constraints on the motion of the object caused by the obstacles 
correspond to surfaces in the configuration space for the object. In this paper we 
will describe a representation of rotation which simplifies the constraints to purely 
algebraic form. We will describe an efficient intersection test for an object which 
is translating uniformly (in a straight line) among obstacles and whose rotation is 
almost uniform, which corresponds to uniform motion in its configuration space. 

A three-dimensional solid object has three translational and three rotational 
degrees of freedom. Its orientation is typically specified using Euler angles, but this 
gives configuration space constraints which contain transcendental functions [6]. 
More recently, Donald [4] described a 3-d path planning algorithm which used angular 
representation for rotation. Schwartz and Sharir [7] described a representation for 
rotation using the coefficients of a 3x3 rotation matrix, giving algebraic constraints. 
However, this approach leads to a twelve dimensional configuration space, and 6 
additional constraints must be added to ensure that all configurations lie on a 
6-manifold corresponding to valid rotation matrix coefficients. 

The orientation of the object may be represented using a four component 
quaternion [5]. This gives rise to a 7 dimensional configuration space, with one 
additional constraint to ensure that the quaternion has unit magnitude. Since 
rotation of a vector by a quaternion involves only multiplication and addition, 
the constraints will again be algebraic. In fact it is possible to eliminate the extra 
variable by using a scaling property of quaternions. This step does not increase the 
degree of the constraints as would, for example, elimination of the extra variable 
using resultants. 

We then describe a procedure for detecting whether a moving polyhedral object 
collides with polyhedral obstacles, and for finding the collision points. Boyse [1] 
treated separately the cases of uniform translation and uniform rotation. In the 
implementation we describe, the object moves with both uniform translation and 
uniform rotation through the configuration space , which corresponds approximately 
to uniform rotation in real space. By expressing each coordinate of the object as a 
polynomial function of some parameter that varies along the path, it is possible to 
detect collisions along more general paths. 

The paper is organised as follows: In section 2 we derive the three types of 
constraint between a polyhedral object and polyhedral obstacles, and show that 
they are algebraic if a quaternion representation of rotation is used. In section 3 
we derive a slightly different representation of rotation which uses only 3 rotational 
parameters (as opposed to 4 for quaternions). This representation is derived by 
projecting the 3-sphere of unit quaternions onto a hyperplane. We show that with 
this representation the constraints have degree 2 as multinomials in the rotation 
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parameters. In section 4 we derive a predicate which defines the configuration space 
obstacles, that is, it delimits those regions of configuration space which correspond 
to interference between object and obstacle. The predicate is constructed from 
unions and intersections of inequalities in the constraint polynomials. In section 

5 we present an intersection test for an object that is moving in a straight line 
in configuration space, which corresponds roughly to the object translating in a 
straight line in real space and rotating uniformly as it translates. Finally in section 

6 we show how the constraints between a moving object and a stationary obstacle 
can be generalized to deal with two moving objects. We show that the complexity 
of collision detection is no higher if both objects are moving. 


2. Object-Obstacle Constraints 

The interactions between a polyhedral object and obstacles can be divided into 
three classes. We will use the convention of Lozano-Perez (1983) and denote the 
moving object A and the obstacle(s) B. Type-A constraints arise when a vertex 
of B touches a face of A, Type-B when a vertex of A touches a face of B, and 
type-C when an edge of A touches an edge of B. The configuration of the object 
will be given by a position vector x and a four-component quaternion Q. The 
interpretation of a configuration (x, Q) is that the object has been first rotated 
about the origin by Q and then displaced by x. For each type of contact there 
is a corresponding constraint on the configuration of the object, and from the 
surfaces in configuration space defined by the constraints, we derive configuration 
space obstacles. These are regions in the configuration space which correspond to 
interference (overlap) between the object and obstacles. We now derive expressions 
for each type of constraint. 

If the object A is at the origin, then a point y on a face of A must satisfy 
y • — dj^ = 0 where is the unit outward normal of the face and dj^ is the 

distance from the origin to the face in the normal direction. Rotation of the face 
about the origin by Q gives y • Rot(n, 4 , Q) — d^ = 0, where Rot(nyi, Q) denotes 
rotation of the vector by the quaternion Q. Translation of A by x has the same 
effect as a translation of y by —x, so a point lying on the rotated and displaced 
face of A must satisfy (y - x) • Rot(n j4 , Q) - d A = 0. For type-A contact, a vertex 
of the obstacle, p# touches the face of A, so substituting p# for y, we obtain: 

Type A (p b ~ *) * Rot(n^, Q) - dA — 0 (1) 

For type B constraints, we begin with the equation of a face of B which is 

y - n B~du, where n# and d\} are respectively the outward normal and the distance 

from the origin to the face. We then substitute for y an expression for a shifted 
and rotated vertex p a of A, which is just Rot(p^, Q) -I- x, giving: 

Type B (Rot(p,i, Q) + x) • n b ~ dn = 0 (2) 
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For type C constraints, we recall that two non-colinear edges that intersect 
in 3-space define a plane. The normal to this plane can be found from the cross 
product of the two edge directions, and the distance of the plane from the origin is 
the dot product of the normal and any point on one of the edges. More generally, 
we can test whether two edges intersect by finding the equation of the plane passing 
through one of the edges and parallel to the second edge, and testing whether a 
point on the second edge lies in this plane. Suppose the direction of the edge of 
object A (the vector from its tail vertex to its head vertex) is e^, and the direction 
of the edge of the obstacle B is e B , and there is some point p B on the obstacle. 
Then the plane passing through the edge of B and parallel to the edge of A has 
equation (y — Pb) ’ { e A X e B ) = 0. To test for intersection, we substitute for y 
the coordinates of a point on the edge A, p^. Since A has been rotated by Q and 
translated by x, we have in fact y = Rot(pyi, Q) 4- x, and the direction of the edge 
of A will be Rot(eyi, Q). Making these substitutions gives equation (3): 

Type C Rot(e^, Q) X e B • (Rot(p^, Q) + x - p B ) = 0 (3) 

We can expand and rearrange the scalar triple products in (3) to give equation 
(4) which will prove to be a more convenient form for type C constraints: 


Type C Rot(p yi X e^, Q) • e B -F Rot(e,i, Q) X e B • (x — p B ) = 0 (4) 

All these constraints are algebraic, that is, they are polynomial in the 
components of the position vectors and the quaternion Q. We can see this from the 
expansion of the quaternion rotation Rot(y, Q) 

Rot(y, Q) = y[ql - q • q) + 2(y • q)q + 2q 0 q X y (5) 

where qo and q are the scalar and vector parts of the quaternion Q respectively. 
We can now see why equation (4) is prefered over equation (3) as a representation 
for type C constraints. Equation (4) gives polynomials of degree 2 in the quaternion 
components while equation (3) gives polynomials of degree 4. 

We note here that although equation (5) is a natural expression for the rotation 
of a vector using a quaternion, there are alternative forms of (5) which require 
fewer arithmetic operations. Some of these are given in appendix I. These forms 
may be used for more rapid computation of rotations for particular configurations, 
but they do not lead to any simplification of the constraint polynomials. 
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3. A More Efficient Representation of Rotation 

One problem with the constraints presented in the last section is that they 
are polynomial in 4 quaternion components, while an object has only 3 rotational 
degrees of freedom. We now show how one of the components can be eliminated 
without increasing the constraint complexity, and that this new representation is 
related to the old by a certain projection. We will do this first for type C constraints, 
and then for types A and B which will have to be modified slightly. 

An important property of equation (4) for type C constraints is that all terms 
on the right hand side are quadratic in the quaternion components. If the quaternion 
is scaled by some factor a the rotated vector will be scaled by a 2 , i.e. 

Rot(y, aQ) = a 2 Rot(y, Q) 

While we normally think about quaternion rotation in terms of unit quaternions, 
composition with an arbitrary quaternion Q corresponds to rotation by a unit 
quaternion which is Q normalized, followed by scaling by the square of the 
magnitude of Q. In the 7 dimensional space of position -f quaternion components, 
the configuration of an object normally lies on a cylinder which is the direct product 
of the 3-space of positions and the 3-sphere of rotations. If we refer back to the form 
of the type-C constraint, equation (4), we find that scaling the quaternion leaves 
the sign of the left-hand side unchanged. In other words, for fixed position the 
manifold defined by this equation extends throughout the 4-dimensional quaternion 
component space and is radially symmetric about the quaternion origin. If we 
move along some arbitrary path in the 7 dimensional configuration space without 
violating any constraints, the radial projection of this path onto the cylinder of 
unit quaternions will not violate any constraints, and will define a collision-free 
path. 

Given this interpretation of points in the 7-dimensional space, we could try 
planning paths directly in 7-dimensions, but this is very inefficient since known 
algebraic algorithms for this problem are worse than exponential in the number 
of degrees of freedom [7]. A better representation of rotation is a 3-dimensional 
manifold which is not the unit sphere, but a hyperplane. The intersection manifold 
of the hyperplane with the type-C constraint manifold is very simple, and can 
be specified by a multinomial of degree 3, whereas the intersection manifold for 
the sphere requires a multinomial of degree 6. In fact a single hyperplane will 
only project onto one half of the 3-sphere, but this is enough to represent all 
possible rotations since Q and —Q define the same rotation. The only caveat is 
that rotations of exactly 180° correspond to points at infinity on the hyperplane. 

The projection of constraints from the cylinder of normalized quaternions 
to a hyperplane is illustrated schematically in figure 1. This figure shows a 
3-dimensional configuration space consisting of 2 quaternion components and one 
position coordinate, and the projection of a single obstacle onto the hyperplane. 
The hyperplane that we choose is rather arbitrary, and in the collision detector (as 
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Figure 1. Projection of a configuration space constraint from the cylinder of 
normalized quaternions onto a hyperplane. 


in the figure) we have chosen go = 1. For this hyperplane the vector part of the 
quaternion is given by q = n tan | for a rotation about axis n through an angle 

e. 

So far we have shown how to eliminate one quaternion component in the 
equation for type-C constraints with no increase in complexity. The equations for 
type-A and type-B constraints do not exhibit invariance to quaternion magnitude, 
but can be made to by a simple modification. We could rewrite (1) and (2) as a sum 
of terms containing a rotated vector and those that do not. The former terms will 
scale by the square of the quaternion magnitude, but the latter will not. However, 
if we multiply the constant terms by -f q • q = Q • Q which is just the squared 
magnitude, the expression as a whole will again be scale-invariant. 

Type A (p B - x) • Rotfn^, Q) - (Q • Q )d A = 0 (6) 

Type B Rot(p^, Q) • n* + (Q • Q)(x • n B - d B ) = 0 (7) 

Once again these constraints are multinomials of degree 3 in the configuration 
parameters. So the representation of rotation that we will use henceforth is a 3- 
dimensional hyper-plane in the 4-dimensional space of quaternions. A configuration 
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will be represented by position coordinates and the vector quaternion components, 
i.e. as (x, q) assuming qo = 1. By projecting the rotation components of a 
configuration from the hyperplane onto the unit sphere, we can obtain a physically 
realizable configuration which has the same relationship to all constraints. 

This formulation of configuration space constraints for polyhedra would seem 
to be the simplest possible. It requires a 6-dimensional space, and all constraints 
are multinomials of degree 3. In fact all constraints are linear in the translational 
parameters and quadratic in the rotational parameters. It is felt that these 
constraints are simple enough that it will be possible to design algorithms to 
triangulate the algebraic varieties defined by them in reasonable time, and hence 
to provide an effective procedure for the find-path problem. For the present we 
describe a simple collision test for a class of paths through the configuration space. 

4. Testing Overlap Between Object and Obstacle 

In section (2) we presented the configuration space constraints for the three 
types of interaction between polyhedra. It was assumed in that section that 
constraint surfaces correspond to the algebraic varieties defined by the constraints, 
or in other words, to the configurations where the value of a constraint is zero. 
However if we have an arbitrary point in configuration space which does not lie on 
one of the constraint manifolds, we need to know if it corresponds to the object 
being clear of all obstacles (in free space), or to an overlap between the object 
and some obstacle. We need a predicate which indicates whether a configuration 
is “safe” relative to a constraint. This is relatively easy to do for types A and B 
constraints, since both arise between a vertex and a plane, and they depend on a 
normal which points out (or in depending on convention) of a solid object. If we 
assume the normal in (6) and (7) points outward, the left-hand sides of (6) and (7) 
will be positive if the point lies above or outside the surface, and negative if it lies 
below. 

We can now define predicates which test whether a particular configuration is 
safe for A and B constraints. We define a type-A predicate Aij(x, q) to be true if 
the type-A constraint between a face i of A and a vertex j of B is violated , i.e. if 
the left-hand side of (6) is negative. Similarly we define H t y(x, q) to be true if the 
constraint between a vertex i of A and a face j of B is violated. Each predicate 
defines a subset of configuration space in which its constraint is violated, and we will 
use predicates without arguments to denote these sets P — {(x, q) £ 3? 6 |P(x, q) } 
and with c superscripts to denote their complements (the set of points which do 
not violate the constraint) P c = {(x, q) £ 5R 6 |-nP(x, q) }. 

In the following derivation, the configuration obstacles will be defined using 
sets but each can be associated with a particular predicate by making the natural 
link between disjunction of predicates and union of sets etc. We will assume that 
the object and the obstacles are convex. Complicated obstacles (or objects) can be 
broken down into unions of possibly overlapping convex objects relatively cheaply, 
and for collision detection it is unimportant whether obstacles overlap. 
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Because type C constraints do not seem to give a simple inside/outside 
predicate, we need to give a slightly more complicated condition (actually two 
conditions) for type C overlap. We also need the conditions under which type A or 
B violations occur between any features of the object and obstacles. An object A 
and an obstacle B overlap if either (i) a vertex of B lies inside A, (ii) a vertex of A 
lies inside B or (iii) an edge of A pierces a face of B, or (iv) an edge of B pierces 
a face of A. Actually, there are several alternative tests for interference, especially 
for parts (iii) and (iv), see for example Boyse [l]. The j th vertex of B lies inside A 
only if all the type-A constraints between the vertex and faces of A are violated, 
i.e. if Aij(x, q) is true for all i. An overlap of type (i) occurs if any point of B lies 
inside A, and the region of type A violation in configuration space is: 

. U n Ay (8) 

jEvert(B) iEfaces(A) 


Similarly an overlap of type (ii) occurs if any vertex of A lies inside B, which 
corresponds to 


u n B ij (9) 

i£vert(A) jEfaccs(B) 


The test for a violation of type (iii) is not as straightforward because there is 
no simple notion of inside and outside for two edges. We avoid the ambiguity by 
considering an edge of the object and a face of the obstacle. In order for the edge 
to pierce the face, one vertex on the edge must lie above the plane of the face and 
one must lie below. These conditions can be easily tested since they derive from 
type A and B constraints, which have already been computed. The type A and B 
constraints also tell us the direction of the edge relative to the outward normal of 
the face, because if the head of the edge lies above the plane of the face and the 
tail lies below, the dot product of the edge and the outward normal to the face 
must be positive, and vice versa. 

If we compute the type-C constraint between the edge of A and an edge of B 
which bounds the face , we can tell whether the cross product in (3) gives a vector 
that points towards the interior of the face or away from it. From this we can 
determine the sign of (3) that corresponds to interference between the edge and 
the face. If the object edge actually pierces the obstacle face, it must violate all 
the type-C constraints with edges that bound the face. It must also have one of its 
end vertices above and one vertex below the face. This later condition is defined 
by a type B predicate, so the test for interference of an object edge and a face is 
a conjunction of C and type B predicates. The region in configuration space for 
which an edge i of the object A pierces a face j of the obstacle, with the edge 
pointing out of the face, is given by 

%n%n n c it (10) 

kE edgea(j) 
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Where t(t) and h(i) are the head and tail respectively of an edge i of A and k 
ranges over the edges of the j th face of the obstacle. Note that the edges k of the 
face j are assumed to be directed in a counterclockwise chain around the face, so 
that the head vertex of one edge is the tail vertex of the next edge, C^x, q) is true 
if the left hand side of equation (4) is negative for an edge i of A and edge k of 
B. The object edge will be pointing out of the face in this case because the type B 
predicate for the tail of the edge must be true (indicating that the tail lies on the 
obstacle side the face) while the predicate for the head must be false (indicating 
that the head lies outside the obstacle). 

We need to test separately whether the edge pierces the face while pointing 
into it. For the latter test we complement the type B predicates, since the positions 
of the head and tail of the edge are reversed, and complement the type C predicate, 
because the sign of the left-hand side of equation (4) changes when the edge 
direction is reversed. This gives 

n H fl C ik ( U ) 

kEcdgca(j) 

and to test whether the i th edge of A pierces the j th face of B in either direction, 
we simply take the union of (10) and (11). 

A violation of type (iii) will occur if any edge of A pierces any face of B, which 
we can write as 

U (%),yn^ D Cik )u ( B t[i),j n^ H 

iEedgea(A) V kEtdgta(j) ' v kEtdgta{j) 

jEf acee(B) 


Type (iv) constraints closely resemble type (iii), except that type-A constraints 
determine the position of the edge end-points. The expression for a type (iv) 
violation is: 

U ( A j,m ^ ’AjM*) ^ n u n A?,m») ^ H 

iEedgeg(B) V kEedgea{j) ' v kEedgea(j) 

jEfacea(A) 


The object overlaps an obstacle if its configuration is in any one of the regions 
(8), (9), (12) or (13), and so a single predicate can be defined to test for overlap 
which is the disjunction of predicates for (8), (9), (12) and (13). If there is more than 
one object or obstacle, we take an additional disjunction over all object / obstacle 
pairs. Notice that the final predicate has the form of a disjunction of conjunctions, 
and that configuration space obstacles can be thought of as unions of “convex” 
parts (regions defined by a conjunction of constraints). Each of these parts has a 
straightforward geometric interpretation, given below. 
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Each conjunction of type A constraints in equation (8) can be thought of as a 
copy of the object A displaced by the negation of a vector from the origin to some 
vertex of B. This is exactly the volume in 3-space defined by (8) if the orientation 
Q is fixed. As the orientation Q changes, the orientation of the 3-space volume 
changes. Similarly each conjunction in equation (9) defines a copy of the obstacle 
B displaced by the negative of some vertex of A. In this case, the orientation of 
the 3-space volume does not depend on Q but its displacement from the origin will 
change. 

The conjunction of predicates in equation (12) defines a 3-space volume which 
is formed by sweeping a face j of B along an edge i of A. Two of the faces of this 
volume are shifted copies of the face j, and the others are parallelograms whose 
edges are copies of edges of j or the edge i. As orientation is varied the two end 
faces (copies of j) do not change orientation but move relative to each other and 
the volume skews. Finally the conjunction of predicates in (13) defines a 3-space 
volume swept out by a face j of A along an edge i of B. When orientation is 
changed, the orientation of the end faces (copies of j) changes accordingly, but the 
edges joining the two end faces do not change their orientation. 

We note here that although it is necessary to include equations (8), (9), (12) 
and (13) in a test for overlap between object and obstacles, some simplification is 
possible for an algorithm which plans paths between safe configurations. This is 
because if the object starts in a safe configuration, and moves to a configuration 
of overlap, there must be a violation of type (iii) or (iv) somewhere along the path. 
That is, somewhere along such a path an edge of the object must pierce a face 
of the obstacle or vice-versa. In fact any configuration of overlap will include a 
violation of type (iii) or (iv) unless the object lies entirely inside an obstacle, or an 
obstacle is entirely contained in the object. The type (iii) and (iv) constraints define 
in configuration space some connected “free” components, some of which may not 
really be free, but these will not be reachable from a genuine free configuration. 
There are two other pairings of constaint types which happen to be complete from 
the point of view of collision avoidance, namely types (i) and (iii) (any path from 
a free configuration to an unsafe configuration must violate a type (i) or (iii)), and 
also types (ii) and (iv). 

In defining the overlap predicate we have not made use of explicit “applicability 
constraints” for the constraint surfaces [2], and in detail in [4]. The applicability 
constraints delimit ranges of rotations for which a particular type of contact can 
occur. The configuration space obstacle between a convex object and obstacles 
can be expressed as a conjunction of applicable constraints (each of which is 
the disjunction of a constraint and its applicability constraints), instead of a 
disjunction of conjunctions as we have described above. Effectively we have broken 
down object-obstacle interactions into interactions between features (edges, faces, 
vertices) which are simple enough that they are always applicable. Instead we are 
relying on the “redundancy” [4] which is captured naturally in the disjunctive form 
of the constraints. 
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5. An Algorithm for Collision Detection 


If instead of fixed values for x and q we have functions x(s) and q(s) of some 
parameter s, then the predicates Ay( x ><0 e ^ c * become binary-valued functions of 
s, and the disjunction of predicates corresponding to expressions (8),(9),(12),(13) 
defines a binary-valued function which is true for sections of the path where there is 
interference and false otherwise. Dually, we can take the path defined by (x(s), q(s)) 
and intersect it with the union of (8),(9),(12),(13) and we will obtain the subset of 
the path in which there is interference. The true-false transitions of this function 
and of any of the predicates A^s)... correspond to zero-crossings of one of the 
expressions (4), (6) or (7), as functions of the parameter s. Finding the zero-crossings 
of one of these functions gives us the free path segments for the corresponding 
predicate, and we can readily evaluate (8),(9),(12),(13) by computing unions and 
intersections of path segments. 

If the functions x(s) and q(s) are polynomial in the variable s, finding the 
constraint zero-crossings is particularly simple. When we substitute for x and q 
in equations (4), (6) and (7), we obtain univariate polynomials in s, and we can 
apply standard techniques to find their zeros (Collins and Loos [3]). By considering 
a particular path through configuration space we have reduced a problem involving 
polynomials in 6 variables to one involving polynomials in one variable. The 
simplest possible polynomial path is a straight line in configuration space, which 
corresponds to linear motion and approximately uniform rotation in real space. An 
implementation of a collision detector for linear paths is described next. 

Let us assume that the object is at some initial configuration (x,-, q t ) and moves 
to some final configuration (xy,qy). If we assume also that the motion between 
these configurations is uniform in the configuration space, at any intermediate point 
the configuration will be given by (x t - 4- sx^,q t - -j- sq^) where x<f = xj — x t - and 
q d = q j — q t . When we substitute this configuration into (4), (6) and (7) we find 
that all three constraints reduce to univariate cubics in the parameter s. Hence we 
can find closed form solutions for the zeros of each constraint in terms of the cubic 
coefficients. 

To find the complexity of this method, we break the computation into two 
steps. The first step is to find all the zeros of the constraints. Since each constraint 
can have at most 3 zeros, and since there is a simple closed form for the roots of a 
cubic, we assign a fixed cost for the computation of the zeros of one constraint. The 
total number of constraints is N c = EaEb + Fa^B + Eb^A where Fa, Va and Ea 
are respectively the number of faces, vertices and edges of the object A, and Fb, 
Vb and Eb are the corresponding numbers for the obstacles. The time complexity 
of the zero-finding phase is then 0(N e ). 

The second step is the computation of the overlap predicate from the constraint 
predicates (8),(9),(12),(13). It is possible to compute the intersection or union of 
two binary-valued functions in time which is linear in the number of true-false 
transitions if the transitions are ordered in s (by a merge-like operation). We assume 
that a predicate P(s) is represented as a sorted list of values of s which correspond 
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Figure 2. Collision Detection Example, (i) Object starts at A, moves uniformly 
to A !, (ii) collisions points along the path 


to true-false transitions of P(s). To compute the conjunction or disjunction of 
P(s) with another predicate Q(s), we merge their lists of transitions, marking the 
true-false transitions of the result as we go. 

The fastest way to merge all the constraints is to proceed in binary tree fashion 
by first merging pairs of constraints, then taking the outputs of this “phase” 
and merging pairs of them etc. The time required for each phase is linear in 
the total number of transitions of all the predicates being merged in that phase. 
Since merging never introduces new transitions, the total number of transitions at 
each phase will be less than or equal to the number of transitions before the first 
phase. This last number is linear in N e , since each (cubic) constraint has at most 3 
transitions. Each merge phase reduces the number of predicates by a factor of two, 
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so there will be about log 2 N c merge phases, each of which takes time linear in N c . 
Thus the complexity of merging all constraints is 0(N c \ogN c ). 

An example of the operation of the collision detector is given in figure (2). In this 
figure the L-shaped object moves and rotates uniformly between the configurations 
labelled A and A!. The collision detector returns a series of transition points which 
correspond to the object and obstacle just touching. These are illustrated from a 
different angle in the next frame. The algorithm was implemented on a Symbolics 
Lisp Machine and takes about 10 seconds for the problem shown. 

6. Collision Detection for Several Moving Objects 

So far we have assumed that we are given a path for a single object moving 
among fixed obstacles. We can readily extend the interference test to deal with 
several moving objects by testing pairwise for interference between objects. The 
configuration space for 2 objects in motion is 12-dimensional, but note that the 
expressions for overlap in equations (8),(9),(12),(13) are valid independent of the 
configuration space. When the trajectory of both objects is known, the configuration 
parameters are still all functions of a single argument, so once again collision 
detection reduces to solving univariate polynomials. The only significant difference 
is in the constraint equations themselves, and we now give the general form for A, 
B and C constraints between moving objects. 

We denote one of the moving objects A and the other B, and let the configuration 
of A be (x A , Q a), and that of B be (x B , Q B ) (we will assume normalized quaternions 
for the moment). Recall that the equation for type-A interaction with B fixed is 

Type A (pb - x^) • Rot(n A , Q^) - d A = 0 

But if B has configuration (x^Q#), then the actual position of p B will be 
Rot(pff, Qjg) + x B , and the generalized type-A constraint is 


Generalized Type A (Rot(p fl , Q B ) + x B - x A ) • Rot(n,i, Q^) — d A = 0 (14) 

We can again use projection of quaternions to eliminate one dimension of 
the configuration space, and we do this independently for and Q#. We first 
make equation (14) independent of quaternion magnitude. There is a term in (14) 
which is the dot product of two rotated vectors, and the magnitude of this term 
will vary with the product of the squared magnitudes of the two quaternions. To 
make the equation independent of magnitude, we must multiply the other terms 
by the squared magnitudes, which are given by dot products of the quaternions 
with themselves. The scale independent form is 

Generalized Type A 

(R ot(p B ,Q B )-^(Q iB -Q B )(x B -x A ))’Rot(n Af Q A )-(Q A -Q A )(Q iir Q B )d A = 0 (15) 
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If both A and B are moving, type A and B constraints are of the same form, 
since both arise when a vertex of one object touches a face of the other. By 
symmetry, the equation for generalized type B interaction can be obtained from 
(15) by simply interchanging A and B subscripts. 

Generalized Type B 

(Rot(p^,Q y i) + (QA-Qyi)(x J 4-XB))-Rot(n 5 ,QB)-(QA-QA)(QB-QBH J B = 0 (16) 

For type C constraints we take equation (3) for edge-edge interaction for a stationary 
object B 

Type C Rot(e^, Qa) X e# • (Rot(pA, Qa) + xa - p b) = 0 

and rotate the edge e# by Qb, and rotate and translate the vertex pb- We then 
obtain 

Generalized Type C 

R°t(e J 4, Q^i) X Rot(eB, Qb) * (Rot(pA, Qa) + xa - Rot(pB, Qb) - x B ) = 0 

This form is not scale independent, but by rearranging scalar triple products and 
using the distributivity of rotation over cross product, we obtain 

Generalized Type C 

Rot(pA X eA, Qa) • Rot(eB, Qb) — Rot(pB X eB, Qb) * Rot(eA, Qa) , 17 x 
4-Rot(e a, Qa) X Rot(eB, Qb) * (xa — x B ) = 0 

and this equation is independent of the magnitude of either quaternion. Thus we 
can represent rotations using the vector parts of the quaternions only i.e. qA and 
qB, and assume the scalar parts have value 1. 

For motion planning, the configuration space is now 12-dimensional and 
configurations are of the form (xa, qA»XB,qo)- The configuration obstacles in this 
space are again defined by the union of (8),(9),(12),(13), but where the A, B 
and C constraints are the generalized scale-independent constraints (15),(16),(17) 
instead of the original constraints (4),(6),(7). The new constraints are algebraic but 
their total degree is now 5 instead of 3. Each constraint is linear in the position 
components of the configuration and quartic in the rotation components. 

The collision detection scheme described in section 5 can be readily extended 
to deal with the generalized constraints. Suppose the two objects move uniformly 
from configuration (xAi, qAtjX/j,-, qBt) 1° configuration (xa/, qA/> x B/, q£/)- Each 
component of the objects’ configuration will be a linear function of some parameter 
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s, and since the total degree of the constraints is 5, each constraint will be a quintic 
polynomial in the parameter s. Quintics can not be solved explicitly, but numerical 
methods such as the modified Uspensky algorithm [3] can be used to find all the 
zeros of a quintic very rapidly. The zeros of the quintics correspond to true-false 
transitions of predicates, and these can be merged exactly as described in section 
5, and the time bounds derived there are still valid. 

Thus collision detection for pairs of moving objects has the same asymptotic 
complexity as collision detection for a moving object and a fixed obstacle. The ratio 
of running times for the two methods is expected to be small (less than an order of 
magnitude) although this has not been verified in an implementation. For several 
moving objects, we find ranges of values of s which lead to collisions between pairs 
of objects, and then find the union of these ranges for all pairs. There are 0(n 2 ) 
such pairs to be tested for n moving objects, giving similar complexity to the fixed 
obstacle scheme when the object and obstacle are built from n convex parts. 


7. Conclusions 


In this paper we described the three basic types of interaction between polyhedral 
objects and obstacles, and derived equations for each type of interaction. We showed 
that the use of quaternions to represent three-dimensional rotations gives rise to 
algebraic constraints between object and obstacles. We gave a projection of the 
3-sphere of unit quaternions onto a 3-dimensional hyperplane in the 4 dimensional 
quaternion space as an alternative representation of rotations that reduces the 
complexity of the configuration space constraints, after slight modification of some 
of the constraints. The new form of the constraints was given for types A, B and C 
interactions. They were found to be linear in the position coordinates and quadratic 
in the rotation components. 

We then derived logical expressions for the configuration space obstacles in 
terms of half-spaces defined by constraint inequalities. The configuration obstacle 
for a convex object and obstacles was shown to be a union of intersections of 
half-spaces, or equivalently, as a disjunction of conjunctions of predicates. The 
overlap predicates were divided into 4 types (there was a subdivision of type C 
interaction into 2 types) and although all 4 are necessary to test for overlap, certain 
sets of 2 can be used for path planning between free configurations. 

We showed that if the path of an object can be described by polynomials in 
some parameter, detecting collisions of the object with obstacles requires only the 
solution of a univariate polynomial. We gave an example of collision detection for 
paths which are linear in the configuration space, and found that in this case finding 
contact points required the solution of univariate cubics. The intersection/union of 
path segments was accomplished by merging sorted transition lists, and the total 
time for the algorithm was shown to be almost linear in the number of constraints. 

The moving object / fixed obstacle constraints were then generalized for two 
moving objects. This lead to constraints that were polynomials of total degree 5 in 
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the configuration parameters. We outlined a collision detection scheme for a pair of 
uniformly moving objects and showed that it had the same asymptotic complexity 
as the fixed obstacle scheme, the main difference being that it required the solution 
of quintic rather than cubic polynomials. 

Natural extensions of the work would deal with more general polynomial 
paths, or with different sets of algebraic constraints, such as those for a 6 degree 
of freedom manipulator. The ultimate objective of this work is to apply techniques 
from computer algebra to design a triangulation procedure for algebraic constraints, 
which would enable us to find collision-free paths through the configuration space. 
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Appendix I 

Equation (5) is the form most commonly used for computing the composition 
of a vector and a quaternion. As given it requires a total of 22 multiplications and 
14 additions, plus 2 shift operations for scalar multiplication by 2. If the quaternion 
is assumed to have unit magnitude, i.e. <?q -f q * q = (5) becomes 

Rot(y, Q) = y(2 ql - 1) + 2(y • q)q + 2q Q q X y (0 

which requires 19 multiplications and 12 additions by saving on a scalar product. 
We can obtain a greater saving by noting that the two scalar products in (5) form 
the expansion of a vector triple product. When these are combined we obtain 

Rot(y, Q) = y(<Zo + q ■ q) + 2q X (q X y) + 2q 0 q X y («') 

which at first sight does not seem to be much of a simplification. However, the 
vector product q X y occurs twice in (ii) and need only be computed once. This form 
requires 19 multiplications and 15 additions, and works for arbitrary quaternions. 
If we assume that the quaternion Q has unit magnitude we obtain finally 


Rot(y, Q) = y + 2q X (q X y) + 2q 0 q X y («0 

and this form requires only 15 multiplications and 12 additions, a saving of 4 
multiplications over (i). 

In fact if multiplications are a serious expense, we can reduce their number still 
further. We have assumed in the above analysis that vector cross product requires 
6 multiplications and 3 additions. It can actually be done with 5 multiplications 
and some extra additions. Suppose u = v X w, and denote the x-component of u 
as u x etc. We compute the following quantities 

( Vy + V z )(w z -Wy) = a ( V y - V z )(w z + Wy) = (3 

(v z + v x )(w x -w z ) = 7 ( v 2 - v x )(w x + w z ) — 6 

{v x - Vy)(w y + w x ) = e 


and the vector u is given by 


u x — 


a- 1 - 
2 


U, 


7 + S 
2 


—a + p — 7 + S 

U 2 = € + --- 


This form of cross product requires 5 multiplications but 16 additions, compared 
with 6 multiplies and 3 additions for the conventional method. It is only likely to 
be useful when multiplication is very expensive compared to addition. 
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